#include "switch.h"
#include "modbus.h"

type_switch switch_data = {0x23};

void switch_init(void)
{
  GPIO_Init(SWITCH_K0_PIN, GPIO_MODE_OUT_PP_HIGH_FAST);
  GPIO_Init(SWITCH_K1_PIN, GPIO_MODE_OUT_PP_HIGH_FAST);
  GPIO_Init(SWITCH_V0_PIN, GPIO_MODE_OUT_PP_HIGH_FAST);
  GPIO_Init(SWITCH_V1_PIN, GPIO_MODE_OUT_PP_HIGH_FAST);
  
  GPIO_Init(SWITCH_H1_PIN, GPIO_MODE_IN_FL_NO_IT);
  GPIO_Init(SWITCH_H2_PIN, GPIO_MODE_IN_FL_NO_IT);
  GPIO_Init(SWITCH_H3_PIN, GPIO_MODE_IN_FL_NO_IT);
}

void switch_km(uint8_t value)
{
  switch(value)
  {
  case 0x01: GPIO_WriteLow(SWITCH_K0_PIN);      GPIO_WriteLow(SWITCH_K1_PIN);   break;
  case 0x02: GPIO_WriteHigh(SWITCH_K0_PIN);     GPIO_WriteLow(SWITCH_K1_PIN);   break;
  case 0x03: GPIO_WriteLow(SWITCH_K0_PIN);      GPIO_WriteHigh(SWITCH_K1_PIN);  break;
  case 0x00: GPIO_WriteHigh(SWITCH_K0_PIN);     GPIO_WriteHigh(SWITCH_K1_PIN);  break;
  default: return;
  }
  switch_data.active &= 0xf0;
  switch_data.active |= value;
}

void switch_v(uint8_t value)
{
  switch(value)
  {
  case 0x00: GPIO_WriteHigh(SWITCH_V0_PIN);      GPIO_WriteLow(SWITCH_V1_PIN);   break;
  case 0x03: GPIO_WriteHigh(SWITCH_V0_PIN);      GPIO_WriteHigh(SWITCH_V1_PIN);  break;
  case 0x02: GPIO_WriteLow(SWITCH_V0_PIN);       GPIO_WriteHigh(SWITCH_V1_PIN);  break;
  case 0x01: GPIO_WriteLow(SWITCH_V0_PIN);       GPIO_WriteLow(SWITCH_V1_PIN);   break;
  default: return;
  }
  switch_data.active &= 0x0f;
  switch_data.active |= (value<<4);
}

uint8_t switch_get_input(void)
{
  return switch_data.status;
}

uint8_t switch_get_output(void)
{
  return switch_data.active;
}

void switch_auto(void)
{
  if(switch_data.status&0x01)
  {
    switch_km(1);
    switch_v(1);
    return;
  }
  if(switch_data.status&0x02)
  {
    switch_km(2);
    switch_v(2);
    return;
  }
  if(switch_data.status&0x04)
  {
    switch_km(3);
    switch_v(3);
    return;
  }
  switch_km(0);
  switch_v(0);  
}

void switch_addk(void)
{
  uint8_t k = switch_data.active & 0x0f;
  ++k;
  if(k>3)
  {
    k = 1;
  }
  switch_km(k);
}

void switch_subk(void)
{
  uint8_t k = switch_data.active & 0x0f;
  --k;
  if(k==0 || k>3)
  {
    k = 3;
  }
  switch_km(k);
}

void switch_modbus_read(uint16_t addr, uint8_t len)
{
  addr &= 0x0fff;
  if(addr>2)
  {
    modbus_err(3, 2);
    return;
  }
  if(addr+len>2)
  {
    modbus_err(3, 3);
    return;
  }  
  uint8_t *pc = modbus_txpc();	
  *(pc+1) = 0x03;
  *(pc+2) = len<<1;
  for(uint8_t i=0;i<len;i++)
  {
    *(pc+3+i*2) = 0;
    *(pc+4+i*2) = *(&switch_data.active+addr+i);    
  }
  modbus_write(pc, 3+len*2);
}

void switch_modbus_write(uint8_t *pc)
{
  if(*(pc+1)!=0x20 || *(pc+2) != 0x00)
  {
    modbus_err(6, 2);
    return;
  }
  switch_data.active = *(pc+4);
  switch_v(switch_data.active>>4);
  switch_km(switch_data.active&0x0f);
  modbus_write(pc-1, 6);
}

void switch_modbus(uint8_t *pc)
{
  type_s2c *data = (type_s2c *)(pc+1);
  switch(*pc)
  {
  case 0x03: switch_modbus_read(data->s, *(pc+4));      break;
  case 0x06: switch_modbus_write(pc);                   break;
  default: modbus_err(*pc, 1);
  }
}

void switch_timer(void)
{
  if(++switch_data.timer==100)
  {
    switch_data.status = GPIO_ReadInputData(GPIOC)>>5;
    switch_data.timer = 0;
  }
}